Skip to content

Alternative implementation of multi-group trajectories and update motoman error handling#488

Draft
nardavin wants to merge 4 commits intoros-industrial:noetic-develfrom
MisoRobotics:user/nardavin/feature/multi-group-trajectories
Draft

Alternative implementation of multi-group trajectories and update motoman error handling#488
nardavin wants to merge 4 commits intoros-industrial:noetic-develfrom
MisoRobotics:user/nardavin/feature/multi-group-trajectories

Conversation

@nardavin
Copy link
Copy Markdown

@nardavin nardavin commented Mar 29, 2022 •
edited
Loading

Changes required to reliably control and execute joint trajectories on our multigroup system. This was tested and verified with a Yaskawa multi-group robot with one GP7 arm attached to a rail. This was not tested on a single-group robot to verify backwards compatibility - we suspect a small amount of work is still required to make these changes compatible with a single-group robot.

Changes were motivated by discussion in #450

Notable updates include:

  • Add support for commanding multi-group robots through the joint trajectory action interface.
  • Update the joint trajectory action server to only begin checking for successful trajectory end conditions after the trajectory is expected to end, in order to allow for trajectories with the same start and end position.
  • Propagate motoros errors to the joint trajectory action node to abort the trajectory in case of motoros failure.
  • Add a shutdown handler to verify the robot is disabled on exit.

nardavin added 4 commits March 29, 2022 10:37
@nardavin
Fix motoman drivers to handle multi-group motion
01146c8 
These changes fix incomplete/broken code in the motoman drivers
related to synchronized multi-group trajectory goals. These changes
allow multi-group goals to be sent and correctly handled.
@nardavin
Update end trajectory test to be timer-based
d1cd81a 
This switches how the joint trajectory action detects the end of a
trajectory. Previously, we would wait for the robot to be in-motion
then exit being in-motion. However, this caused stalling issues for
short trajectories. Now, we wait until the time expected to complete
the trajectory has passed and the robot is not in motion.
@nardavin
Abort trajectory actions after motoros errors
c4533d3 
Add functionality in the motoman driver stack for the trajectory
action server to abort the active trajectory when a motoros error is
thrown.
@nardavin
Add motoman shutdown handler to disable arm
f513b1e 
Add a custom shutdown handler to the motoman joint streaming node. The
handler will cancel any active trajectories and disable the arm in the
event of a system shutdown.
@nardavin nardavin mentioned this pull request Mar 29, 2022
Closed
@nardavin nardavin marked this pull request as draft March 29, 2022 17:49
@gavanderhoorn
Copy link
Copy Markdown
Member

gavanderhoorn commented Mar 30, 2022 •
edited
Loading

Thanks for the PR. It includes some interesting ideas.

c4533d3 seems to mimic ros-industrial/industrial_core#271, but makes it Motoman-specific.

d1cd81a is a nice idea, but on my test setup (YRC1000, 4 groups) results in all goals to always be ABORTED with:

[ WARN] [...] [/arm_controller/follow_joint_trajectory]: Outside goal constraints, aborting trajectory
[ INFO] [...] [/arm_controller/follow_joint_trajectory]: Aborting active goal.
[ INFO] [...] [/motion_streaming_interface]: Receiving joint trajectory message Dynamic
[ INFO] [...] [/motion_streaming_interface]: Empty trajectory received, canceling current trajectory
[ INFO] [...] [/move_group]: Controller 'arm_controller' successfully finished
[ WARN] [...] [/move_group]: Controller handle arm_controller reports status ABORTED
[ INFO] [...] [/move_group]: Completed trajectory execution with status ABORTED ...
[ INFO] [...] [/move_group]: Execution completed: ABORTED

f513b1e is also a nice idea. I'm wondering though, similar code is include here:

motoman/motoman_driver/src/industrial_robot_client/joint_trajectory_interface.cpp

Lines 192 to 196 in 7860ff5

JointTrajectoryInterface::~JointTrajectoryInterface()
{
trajectoryStop();
this->sub_joint_trajectory_.shutdown();
}

I guess you observed that dtor to not always successfully disable trajectory mode?

@gavanderhoorn gavanderhoorn changed the title Add support for multi-group trajectories and update motoman error handling Alternative implementation of multi-group trajectories and update motoman error handling Mar 30, 2022
@nardavin
Copy link
Copy Markdown
Author

nardavin commented Mar 30, 2022

Thanks for the PR. It includes some interesting ideas.

c4533d3 seems to mimic ros-industrial/industrial_core#271, but makes it Motoman-specific.

d1cd81a is a nice idea, but on my test setup (YRC1000, 4 groups) results in all goals to always be ABORTED with:

[ WARN] [...] [/arm_controller/follow_joint_trajectory]: Outside goal constraints, aborting trajectory
[ INFO] [...] [/arm_controller/follow_joint_trajectory]: Aborting active goal.
[ INFO] [...] [/motion_streaming_interface]: Receiving joint trajectory message Dynamic
[ INFO] [...] [/motion_streaming_interface]: Empty trajectory received, canceling current trajectory
[ INFO] [...] [/move_group]: Controller 'arm_controller' successfully finished
[ WARN] [...] [/move_group]: Controller handle arm_controller reports status ABORTED
[ INFO] [...] [/move_group]: Completed trajectory execution with status ABORTED ...
[ INFO] [...] [/move_group]: Execution completed: ABORTED

I suspect something more subtle is going on here. Do the trajectories succeed on 01146c8? Not sure how adding the timer constraint would cause the trajectories to be aborted.

f513b1e is also a nice idea. I'm wondering though, similar code is include here:

motoman/motoman_driver/src/industrial_robot_client/joint_trajectory_interface.cpp

Lines 192 to 196 in 7860ff5

JointTrajectoryInterface::~JointTrajectoryInterface()
{
trajectoryStop();
this->sub_joint_trajectory_.shutdown();
}

I guess you observed that dtor to not always successfully disable trajectory mode?

Correct - I'm not sure what was going on here, but adding the explicit sigterm handler was more reliable in shutting down the arm after killing the rosnode.

@akashjinandra
Copy link
Copy Markdown

akashjinandra commented Apr 4, 2022

This has been tested on our setup on a YRC1000 with 4 robot groups. Thanks for all your hard work!

@akashjinandra akashjinandra mentioned this pull request Apr 4, 2022
Closed
@rsinnet
Copy link
Copy Markdown

rsinnet commented Apr 11, 2022

This has been tested on our setup on a YRC1000 with 4 robot groups. Thanks for all your hard work!

Wow, thanks for the hardware testing!

@gavanderhoorn gavanderhoorn mentioned this pull request Aug 12, 2022
Open
@gavanderhoorn gavanderhoorn mentioned this pull request Feb 26, 2024
Closed
@riv-mjohnson riv-mjohnson mentioned this pull request Feb 29, 2024
Closed
@gavanderhoorn gavanderhoorn changed the base branch from kinetic-devel to noetic-devel April 15, 2025 11:22
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Reviewers

No reviews

Assignees

No one assigned

Labels

None yet

Milestone

No milestone

Development

Successfully merging this pull request may close these issues.

4 participants

@nardavin @gavanderhoorn @akashjinandra @rsinnet